#include "programset.h"
#include "fanucparseclass.h"
#include "programtexttransfer.h"
#include "frames.hpp"
#include <Eigen/Dense>

FanucParseClass::FanucParseClass()
{

}

int FanucParseClass::parseToNulanguage(QString &incomeProgram, QStringList &cmdStrListOut,
                                       QVector<PointPro> &pointListOut, QStringList &messageOut)
{

}

int FanucParseClass::parseToGcode(QString &strFilePath, QString &gcodeOut, QStringList &messageOut)
{
//    std::vector<double> pointIn;
//    pointIn.resize(6);
////    pointIn[3]=25;
////    pointIn[4]=72;
////    pointIn[5]=63;
//    pointIn[3]=-62;
//    pointIn[4]=72;
//    pointIn[5]=-63;
//    rpy2Zxz(pointIn,pointIn);
//    return 1;
    ProgramTextTransfer programFransfer(1);
    QVector <PlRegStruct> regPlList_out;//测试放到文件里。　todo
    QVector <StackInfo> palletStackInfo_out;
    QVector <PointPro> pointList_out;
    QMap <int, DPoint> vehicleDPointListOut;
    QString tmpVersion;
    QVector <QString> cmdStrList;

    int tmpKey=programFransfer.transferText(strFilePath,cmdStrList,pointList_out,regPlList_out,palletStackInfo_out,
                                 vehicleDPointListOut,tmpVersion);

//    ProgramSet* programSet=new ProgramSet;//一个任务一个程序集
    ProgramSet programSet;//一个任务一个程序集
    int endFlag = programSet.transferTextInstructionToStructData(cmdStrList,0,"test",0,0);
    gcodeOut.clear();
    QString tmpLine;
    for(int i=0;i<programSet[0].size();i++)
    {

        transToGcode(programSet[0][i],pointList_out,tmpLine);
        gcodeOut+=tmpLine;
    }

    gcodeOut+="M30";

    return 1;

}

int FanucParseClass::transToGcode(Line &oneLine,QVector <PointPro> &pointListIn, QString &gcodeOut)
{
    if(MOTION!=oneLine.type)
    {
        return 0;
    }
    PointPro tmpPoint;
    tmpPoint=pointListIn[oneLine.motion.p-1];

//    KDL::Rotation tmpRatation=KDL::Rotation::RPY(tmpPoint.positionValue[3]*M_PI/180.0,
//                            tmpPoint.positionValue[4]*M_PI/180.0,
//                            tmpPoint.positionValue[5]*M_PI/180.0);
//    double tmpa,tmpb,tmpc;
//    tmpRatation.GetEulerZYZ(tmpa,tmpb,tmpc);
//    tmpRatation.GetEulerZYX(tmpa,tmpb,tmpc);
//    //宝元欧拉角使用ZXZ定义

//    tmpPoint.positionValue[3]=180.0*tmpa/M_PI;
//    tmpPoint.positionValue[4]=180.0*tmpb/M_PI;
//    tmpPoint.positionValue[5]=180.0*tmpc/M_PI;

    rpy2Zxz(tmpPoint.positionValue,tmpPoint.positionValue);

    gcodeOut="G1";
    gcodeOut+=" X"+QString::number(tmpPoint.positionValue[0],'f',2);
    gcodeOut+=" Y"+QString::number(tmpPoint.positionValue[1],'f',2);
    gcodeOut+=" Z"+QString::number(tmpPoint.positionValue[2],'f',2);
    gcodeOut+=" A"+QString::number(tmpPoint.positionValue[3],'f',2);
    gcodeOut+=" B"+QString::number(tmpPoint.positionValue[4],'f',2);
    gcodeOut+=" C"+QString::number(tmpPoint.positionValue[5],'f',2);
    gcodeOut+=" F"+QString::number(oneLine.motion.rate*60);
    gcodeOut+="\n";

    return 1;

}

int FanucParseClass::rpy2Zxz(std::vector<double> pointIn, std::vector<double> &pointOut)
{
        Eigen::Vector3d rpy_raw, ypr;
        rpy_raw << pointIn[3], pointIn[4], pointIn[5];//x,y,z
        rpy_raw = rpy_raw * M_PI / 180;


        Eigen::Matrix3d R_AB;

        //输入部分为YPR,所以转换的输出也是YPR
        R_AB = Eigen::AngleAxisd(rpy_raw[2], Eigen::Vector3d::UnitZ())
                * Eigen::AngleAxisd(rpy_raw[1], Eigen::Vector3d::UnitY())
                * Eigen::AngleAxisd(rpy_raw[0], Eigen::Vector3d::UnitX());

//        R_AB = Eigen::AngleAxisd(1.57, Eigen::Vector3d::UnitZ())
//                * Eigen::AngleAxisd(0.78, Eigen::Vector3d::UnitY())
//                * Eigen::AngleAxisd(0.78, Eigen::Vector3d::UnitX());

//        ypr = R_AB.eulerAngles(2, 1, 0);//rpy
        ypr = R_AB.eulerAngles(2, 0, 2);//zxz
//        ypr = R_AB.eulerAngles(2, 1, 2);//zyz

        pointOut=pointIn;
//        pointOut[3]=ypr[2]*180.0/M_PI;
//        pointOut[4]=ypr[1]*180.0/M_PI;
//        pointOut[5]=ypr[0]*180.0/M_PI;
        pointOut[3]=ypr[0]*180.0/M_PI;
        pointOut[4]=ypr[1]*180.0/M_PI;
        pointOut[5]=ypr[2]*180.0/M_PI;
        return 1;

}
